/*
  rcsteering.c
  Controls the Steering function

  Created by:  Andrew Stiegmann
  
 */

#include "rcsteering.h"
#include <pwmout.h>
#include <error.h>
#include <rtos.h>

#define RCSTEERING_MIDDLE  95

#define MAX_LEFT 109 
#define MIN_LEFT (RCSTEERING_MIDDLE)
#define DELTA_LEFT (MAX_LEFT - MIN_LEFT)

#define MAX_RIGHT 74 
#define MIN_RIGHT (RCSTEERING_MIDDLE)
#define DELTA_RIGHT (MAX_RIGHT - MIN_RIGHT)  

#define RCSTEERING_PWM_CHANNEL 1

#define RCSTEERING_COUNT 1

#define RC_DIVAMUX 4
#define RC_DIVAVAL 44

// Warning: Keep this value to an absolute minimum.
// It will slow down the program unless the steering control
// is in its own thread.
#define RCSTEERING_SLEEPTIME 500

int RCSteering_Init( void );
int RCSteering_DeInit( void );
inline int RCSteering_SetPWM( int val );

struct _RCSteering
{
  int users;
  int direction;
} RCSteering;

int RCSteering_SetActive(int state)
{
  if ( state )
    return RCSteering_Init( );
  else
    return RCSteering_DeInit( );
}

int RCSteering_GetActive( void )
{
  return RCSteering.users;
}

int RCSteering_SetDirection(int val)
{
  // Sets the RCSteering Speed based on a percentage
  // Left = -100%, Right = 100%
  int status;

  if ( val < -100 || val > 100 )
    return CONTROLLER_ERROR_ILLEGAL_PARAMETER_VALUE;
  
  if ( !RCSteering.users )
    {
      status = RCSteering_Init();
      if ( status != CONTROLLER_OK )
	return status;
    }

  // Now attempt to set the Steering values
  if ( val > 0 )
    {
      // We set a Right Direction
      status = RCSteering_SetPWM( val * DELTA_RIGHT / 100 + MIN_RIGHT );
    }
  else if ( val < 0 )
    {
      // We set a Left Direction
      val *= -1;
      status = RCSteering_SetPWM( val * DELTA_LEFT / 100 + MIN_LEFT );
    }
  else
    {
      status = RCSteering_SetPWM(RCSTEERING_MIDDLE);
    }

  if( status != CONTROLLER_OK )
    return status;
  
  RCSteering.direction = val;
  return CONTROLLER_OK;
}

int RCSteering_GetDirection()
{
  return RCSteering.direction;
}

int RCSteering_Init()
{
  int status; 

  // Check if it already has a user
  if ( RCSteering.users )
    return CONTROLLER_ERROR_CANT_LOCK;
  
  // Start that channel
  status = PwmOut_SetActive( RCSTEERING_PWM_CHANNEL, 1 );

  if ( status < 0 )
    return status;

  PwmOut_SetDividerAValue( RC_DIVAVAL );
  PwmOut_SetDividerAMux( RC_DIVAMUX );
  RCSteering.users++;
  RCSteering.direction = 0;
  return RCSteering_SetPWM(RCSTEERING_MIDDLE);
  
}

int RCSteering_DeInit()
{ 
  // Check if it already has a user
  if ( !RCSteering.users )
    return CONTROLLER_ERROR_TOO_MANY_STOPS;
  
  // Stop that channel
  RCSteering.users--;
  RCSteering.direction = 0;
  return PwmOut_SetActive( RCSTEERING_PWM_CHANNEL, 0 );
  
}

inline int RCSteering_SetPWM(int duty)
{
  //int stat;
  // Sets the RCSteering Speed Directly
  // For staters, we set the direction with the motor
  return PwmOut_SetDuty( RCSTEERING_PWM_CHANNEL, duty );
  /*
  if ( stat != CONTROLLER_OK )
    return stat;

  // Then we sleep for a bit to allow the motor to catch up
  Sleep(RCSTEERING_SLEEPTIME);

  // Now we turn off the Pulses to the Steering Controler
  PwmOut_SetDuty( RCSTEERING_PWM_CHANNEL, 0 );
  if ( stat != CONTROLLER_OK )
    return stat;
 
  // If everything is ok here, return ok
  return CONTROLLER_OK;
  */ 
}
  

#ifdef OSC

/** \defgroup PWMOutOSC PWM Out - OSC
  Control the RCSteering via OSC.

*/

#include "osc.h"
#include "string.h"
#include "stdio.h"

// Need a list of property names
// MUST end in zero
static char* RCSteeringOsc_Name = "rcsteering";
static char* RCSteeringOsc_PropertyNames[] = { "active", "direction", 
					       0 }; // must have a trailing 0

int RCSteeringOsc_PropertySet( int index, int property, int value );
int RCSteeringOsc_PropertyGet( int index, int property );

// Returns the name of the subsystem
const char* RCSteeringOsc_GetName( )
{
  return RCSteeringOsc_Name;
}

// Now getting a message.  This is actually a part message, with the first
// part (the subsystem) already parsed off.
int RCSteeringOsc_ReceiveMessage( int channel, char* message, int length )
{
  int status = Osc_IndexIntReceiverHelper( channel, message, length, 
                                     RCSTEERING_COUNT, RCSteeringOsc_Name,
                                     RCSteeringOsc_PropertySet, RCSteeringOsc_PropertyGet, 
                                     RCSteeringOsc_PropertyNames );
                                     
  if ( status != CONTROLLER_OK )
    return Osc_SendError( channel, RCSteeringOsc_Name, status );
  return CONTROLLER_OK;
}

// Set the index LED, property with the value
int RCSteeringOsc_PropertySet( int index, int property, int value )
{
  index = 0; //To eliminate stupid Compiler Warning
  switch ( property )
  {
    case 0: 
      RCSteering_SetActive( value );
      break;      
    case 1:
      RCSteering_SetDirection( value );
      break;
  }
  return CONTROLLER_OK;
}

// Get the index LED, property
int RCSteeringOsc_PropertyGet( int index, int property )
{
  index = 0; //To eliminate stupid Compiler Warning
  int value = 0;
  switch ( property )
  {
    case 0:
      value = RCSteering_GetActive( );
      break;
    case 1:
      value = RCSteering_GetDirection( );
      break;
  }
  return value;
}


#endif
